// 采用多方向采样的树搜索避障策略,基于PCL库的最邻近搜索算法
#ifndef COLLISIONCHECKER_H
#define COLLISIONCHECKER_H

#include <iostream>
#include <cmath>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

namespace Planner
{
    using std::cout;
    using std::endl;
    using std::vector;
    using std::string;
    using Eigen::Vector3d;
    using Eigen::MatrixXd;
    using Eigen::Quaterniond;

    class CollisionChecker
    {
    public:
        // FOV相关参数默认为90，表示默认情况下不进行重规划点在FOV内的判断，需要显示传递参数已开启该功能
        void setParams(double secure_dis, int iter_num, double hangle = 90, double vangle = 90);

        void setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr &tree);

        void setCurrPose(const Eigen::Vector3d &pose, const Eigen::Quaterniond &orien);

        bool isFeasible(const Eigen::Vector3d &waypt, double &dis);

        // 沿多个方向生长树节点，选取最佳节点位置
        bool replanByTree(const Vector3d &waypt, const Vector3d &direction, Vector3d &revise_pt);

    private:
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;
        Eigen::Vector3d near_pt_;
        Eigen::Vector3d curr_pose_;
        Eigen::Quaterniond curr_orien_;
        double secure_dis_;  // 安全距离
        double near_dis_;
        int iter_num_;           // 迭代次数
        double vertical_angle_;   // 竖直方向视野角
        double horizontal_angle_; // 水平方向视野角

        // 采用kdtree进行邻近点搜索
        bool nearSearch(const Eigen::Vector3d &pt);

        // 判断重规划后的路径点是否在雷达点云的视野内
        bool insideFOV(const Eigen::Vector3d &pt);

        // 计算到障碍物的距离
        double calcDis(const Vector3d &waypt);
    };
} // namespace Planner
#endif